Positional estimation method using one-step movements and an inertial navigation system

ABSTRACT

A method for estimating the position of a mobile device. Initializing an Extended Kalman Filter. Moving mobile device from a first unknown position (xt1, yt1) at a first time t1 to a second unknown position (xt2, yt2) at a second time t2. Measuring distance traveled d and angle traveled θ between the first unknown position and the second unknown position. Calculating a first possible position (x1, y1) at the first time and a second possible position (x2, y2) the second time. Using the Extended Kalman Filter, a predicted first unknown position ({circumflex over (x)}, ŷ) is calculated at the first time. A first final value is calculated according to √{square root over (({circumflex over (x)}−x1)2+(ŷ−y1)2)}. A second final value is calculated according to √{square root over (({circumflex over (x)}−x2)2+(ŷ−y2)2)}. If the first final value is less than or equal to the second final value, the first possible position (x1, y1) is output. Otherwise, the second possible position (x2, y2) is output.

FEDERALLY SPONSORED RESEARCH AND DEVELOPMENT

The Positional Estimation Method Using One-Step Movements and anInertial Navigation System is assigned to the United States Governmentand is available for licensing and commercial purposes. Licensing andtechnical inquiries may be directed to the Office of Research andTechnical Applications, Space and Naval Warfare Systems Center Pacific(Code. 72120), San Diego, Calif., 92152 via telephone at (619) 553-2778or email at ssc_pac_t2@navy.mil. Reference Navy Case 103546.

BACKGROUND OF THE INVENTION 1. Field of the Invention

The present invention relates generally to a mobile device locationsystem, and in particular to a method for effectively tracking thelocation of a mobile device using an inertial navigation system onboardthe device and receiving and measuring signals from a transmitter with aknown location.

2. Description of the Related Art

A navigation system provides the user with an estimate of position andorientation. Global Navigation Satellite Systems (GNSS) such as theGlobal Positioning System (GPS) are designed to assist users inestimating those quantities (e.g., position and orientation). However,GNSS requires line of sight from the satellite to the user in order tobe useful in providing reliable estimates. Therefore, in order toincrease the robustness of navigation systems, GNSS receivers can beintegrated with other navigation sensors such as inertial navigationsystems (INS) or other radio frequency (RF) systems of opportunity (suchas cellular telephone signals or digital television signals).

An INS provides the user with specific force measurements (which areused to estimate acceleration vectors) and rotation rates (which areused to estimate position and orientation) at higher data rates than theGNSS. However, by itself, INS suffers from errors which increase overtime, and requires regular updates from GNSS or RF signals ofopportunity in order to prevent errors from growing. An RF signal ofopportunity provides the user with positional information using rangemeasurements to an RF transmitter at a known location. As RFtransmitters are located on the surface of the earth, their signalstrengths will tend to be higher and their signals more reliable thanGNSS signals (which are weaker and transmitted from space), especiallyif the receiver is indoors.

Typically, an Extended Kalman Filter (EKF) is used to filter data fromthe GNSS, RF signals of opportunity, and the INS. The EKF is defined byits state vector, time update, and measurement update. The state vectorcontains the parameters for which estimation is desired. The time updateis how the state vector evolves with time using a dynamic model of thesystem, and is referred to as the a priori state estimate. Themeasurement update is used to optimally combine the current measurementwith the a priori state prediction, thus improving the state vector.Examples of how the EKF is used to filter and integrate GNSS, RF signalsof opportunity, and INS data are described by Scott Gleason et al. inGNSS Applications and Methods, Artech House, 2009, pp. 149-75, which isherein incorporated by reference in its entirety.

SUMMARY OF THE INVENTION

The present invention is a method for estimating the position of amobile device. An Extended Kalman Filter is initialized at the mobiledevice. The mobile device is physically moved from a first unknownposition (x_(t1), y_(t1)) at a first time t₁ to a second unknownposition (x_(t2), y_(t2)) at a second time t₂. A distance traveled dbetween the first unknown position and the second unknown position ismeasured at the mobile device. An angle traveled θ between the firstunknown position and the second unknown position is measured at themobile device. A first possible position (x₁, y₁) is calculated at thefirst time. A second possible position (x₂, y₂) is calculated at thesecond time. Using the Extended Kalman Filter, a predicted first unknownposition ({circumflex over (x)}, ŷ) is calculated at the first time. Afirst final value is calculated according to √{square root over(({circumflex over (x)}−x₁)²+(ŷ−y₁)²)}. A second final value iscalculated according to √{square root over (({circumflex over(x)}−x₂)²+(ŷ−y₂)²)}. Upon determining that the first final value is lessthan or equal to the second final value, the first possible position(x₁, y₁) is output as the updated first position (x, y). Upondetermining the first final value is greater than the second finalvalue, the second possible position (x₂, y₂) is output as the updatedfirst position (x, y).

BRIEF DESCRIPTION OF THE DRAWINGS

Throughout the several views, like elements are referenced using likeelements. The elements in the figures are not drawn to scale, and somedimensions may be exaggerated for clarity.

FIG. 1 is a flowchart of a method for estimating the position of amobile device, in accordance with one embodiment of the presentinvention.

FIG. 2 is a flowchart of a method for estimating the position of amobile device, in accordance with one embodiment of the presentinvention, and is a continuation of FIG. 1.

FIG. 3 is a flowchart of a method for estimating the position of amobile device, in accordance with one embodiment of the presentinvention, and is a continuation of FIG. 2.

FIG. 4 is a flowchart of a method for estimating the position of amobile device, in accordance with one embodiment of the presentinvention, and is a continuation of FIG. 1.

FIG. 5 is depicts the relationship between various units andmeasurements used in the present invention.

FIG. 6 depicts a plot of the simulated truth trajectory of an embodimentof the present invention.

FIG. 7 depicts a plot of the simulated estimation error of an embodimentof the present invention.

DETAILED DESCRIPTION OF THE INVENTION

The present invention uses an Extended Kalman Filter (EKF) to filter thedata from an INS and a RF signal of opportunity within a two dimensionalenvironment. The state vector of the EKF is comprised of the following15 parameters: a position error vector (a 3×1 vector in north east down[NED] coordinates); a velocity error vector (a 3×1 vector in NEDcoordinates); an orientation error vector (a 3×1 vector in radians usingthe 3-2-1 set Euler angles); an accelerometer bias error vector (a 3×1vector with units in meters per second squared); and a gyroscope biaserror vector (a 3×1 vector with units of radians per second). The timeupdate phase of the EKF consists of a time update for the accelerometerand gyroscope. The present invention, method 10, is used in themeasurement update of the EKF.

FIG. 1 depicts steps of a method 10 for estimating the position of amobile device (for example, a mobile phone) with an onboard INS. Usingmethod 10, the operator initially initializes an Extended Kalman filterat a mobile device located at a first unknown position (x_(t1), y_(t1)).Step 10 _(a). The mobile device is then moved from the first unknownposition (x_(t1), y_(t1)) at a first time t₁ to a second unknownposition (x_(t2), y_(t2)) at a second time t₂. Step 10 _(b). Because theposition of the transmitter (which may be an RF signal of opportunity,such as a cellular telephone tower or digital television tower) is at aknown location (x_(TX), y_(TX)), the first distance R₁ between thetransmitter and the mobile device at the first unknown position (x_(t1),y_(t1)) is calculated according to equation 1:R ₁=√{square root over ((x _(TX) −x _(t1))²+(y _(TX) −y _(t1))²)}.  (1)

Similarly, the second distance R₂ between the transmitter and the mobiledevice at the second unknown position (x_(t2), y_(t2)) is calculatedaccording to equation 2:R ₂=√{square root over ((x _(TX) −x _(t2))²+(y _(TX) −y _(t2))²)}.  (2)Using the INS onboard the mobile device, the distance traveled d betweenthe first unknown position (x_(t1), y_(t1)) and the second unknownposition (x_(t2), y_(t2)) is measured. Step 10 _(c). Similarly, the INSis used to measure the angle traveled between first unknown position(x_(t1), y_(t1)) and the second unknown position (x_(t2), y_(t2)). Step10 _(d). The relationship between x_(T1), y_(t1), x_(t2), y_(t2),x_(TX), y_(TX), R₁, R₂, and d is depicted in FIG. 5. The INS onboard themobile device can then measure the angle traveled θ between the firstunknown position (x_(t1), y_(t1)) and the second unknown position(x_(t2), y_(t2)). Step 10 _(d).

Once the angle traveled θ between the first unknown position (x_(t1),y_(t1)) and the second unknown position (x_(t2), y_(t2)) is known, themethod determines whether equation 3 below is true, Step 10 _(e):sin θ=0.  (3)If at Step 10 _(e) sin θ is equal to zero, then the first unknownposition (x_(t1), t_(t1)) and the second unknown position (x_(t2),y_(t2)) are calculated according to a method 20 as depicted in FIG. 2.The first step of method 20 is to determine whether equation 4 below istrue, Step 20 _(a):cos θ=1.  (4)If at Step 20 _(a) cos θ is equal to zero, then the next step is tocalculate the x-component of the first possible position (x₁, y₁)according to equation 5:

$\begin{matrix}{x_{1} = {x_{TX} - {\frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2d}.}}} & (5)\end{matrix}$If at Step 20 _(a), cos θ is not equal to zero (or stated alternatively,if equation 4 is false), then the x-component of the first possibleposition (x₁, y₁) is calculated according to equation 6, Step 20 _(c):

$\begin{matrix}{x_{1} = {x_{TX} + {\frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2d}.}}} & (6)\end{matrix}$Next, proceeding after either Step 20 _(b) or Step 20 _(c), thex-component of the second possible position (x₂, y₂) is initializedaccording to equation 7, Step 24

$\begin{matrix}{x_{1} = {x_{TX} + {\frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2d}.}}} & (7)\end{matrix}$After x₁ and x₂ have been calculated, the y-components of the firstpossible position (x₁, y₁) and of the second possible position (x₂, y₂)are calculated according to equations 8 and 9 below, Step 20 _(e) andStep 20 _(f) respectively:y ₁ =y _(TX)+√{square root over (t _(TX) ²−(y _(TX) ²+(x ₁ −x _(TX))² −R₁ ²))};  (8)y ₂ =y _(TX)+√{square root over (t _(TX) ²−(y _(TX) ²+(x ₂ −x _(TX))² −R₁ ²))};  (9)

If at Step 10, sin θ is not equal to zero (or stated alternatively, ifequation 3 is false), then the first possible position (x₁, y₁) and thesecond possible position (x₂, y₂) will be calculated differentlyaccording to a method 40, as depicted in FIG. 4. The first step ofmethod 40 is to calculate the third value a according to equation 10below, Step 40 _(a):a=1+cot²θ.  (10)Next, at Step 40 _(b), the fourth value b is calculated according toequation 11:

$\begin{matrix}{b = {{{- 2}x_{TX}} - {2\;\cot\;{\theta\left( {{{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}} \right)}} + {2{y_{TX} \cdot \cot}\;{\theta.}}}} & (11)\end{matrix}$At Step 40 _(e), the fifth value c is calculated according to equation12:

$\begin{matrix}{c = {x_{TX}^{2} + \left( {{{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}} \right)^{2} - {2{y_{TX}\left( {{{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}} \right)}} + y_{TX}^{2} - {R_{1}^{2}.}}} & (12)\end{matrix}$After the third, fourth, and fifth values a, b, and c have beencalculated, the x-component of the first possible position (x₁, y₁) maybe calculated according to equation 13, Step 40 _(d):

$\begin{matrix}{x_{1} = {\frac{{- b} + \sqrt{b^{2} - {4{a \cdot c}}}}{2a}.}} & (13)\end{matrix}$Next, the x-component of the second possible position (x₂, y₂) may becalculated according to equation 14, Step 40 _(e):

$\begin{matrix}{x_{2} = {\frac{{- b} - \sqrt{b^{2} - {4{a \cdot c}}}}{2a}.}} & (14)\end{matrix}$The y-component of the first possible position (x₁, y₁) is thencalculated according to equation 15, Step 40 _(f):

$\begin{matrix}{y_{1} = {{{{- x_{1}} \cdot \cot}\;\theta} + {{x_{TX} \cdot \cot}\;\theta} + y_{TX} - {\frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}.}}} & (15)\end{matrix}$Then y-component of the second possible position (x₂, y₂) may becalculated according to equation 16, Step 40 _(g):

$\begin{matrix}{y_{2} = {{{{- x_{2}} \cdot \cot}\;\theta} + {{x_{TX} \cdot \cot}\;\theta} + y_{TX} - {\frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}.}}} & (16)\end{matrix}$

After the first possible position (x₁, y₁) and the second possibleposition (x₂, y₂) have been calculated according to either method 20 ormethod 40, the output of the Extended Kalman Filter can be used todetermine a first updated position (x, y) by testing which of the firstpossible position (x₁, y₁) and the second possible position (x₂, y₂) aremore likely to be the actual correct position of the mobile device,method 30 depicted in FIG. 3. The Extended Kalman Filter predicts afirst unknown position ({circumflex over (x)}, ŷ) of the mobile deviceat the first time t₁. Step 30 _(a). Next, a first final value may becalculated according to √{square root over (({circumflex over(x)}−x₁)²+(ŷ−y₁)²)}, Step 30 _(b), and a second final value may becalculated according to √{square root over (({circumflex over(x)}−x₂)²+(ŷ−y₂)²)}, Step 30 _(c). The first final value is thencompared against the second final value according to the inequality setforth in equation 17 below, Step 30 _(d):√{square root over (({circumflex over (x)}−x ₁)²+(ŷ−y ₁)²)}≤√{squareroot over (({circumflex over (x)}−x ₂)²+(ŷ−y ₂)²)}  (17)If the condition of equation 17 is true (if the first final value isless than or equal to the second final value), then the first possibleposition (x₁, y₁) is output as the first updated position (x, y). Step30 _(e). If the condition of equation 17 is false (if the first finalvalue is greater than the second final value), then the second possibleposition (x₂, y₂) is output as the first updated position (x, y). Step30 _(f).

The novel Positional Estimation Method Using One-Step Movements and anInertial Navigation System has been tested in simulations using consumergrade inertial navigation systems for seven minutes using the truthtrajectory depicted in FIG. 6. In FIG. 6, the transmitter is depicted asthe small box labeled “TX” in the upper right quadrant, while the mobiledevice path is depicted as the solid line with “Start” and “Stop” ateither end. In this test, the range measurements from the transmitterhad a measurement accuracy of one meter. Position estimation error forthe x- and y-coordinates during this simulation is depicted in FIG. 7,where the x-axis is the time of simulation in minutes, and the y-axis isthe estimation error of the x- and y-coordinates in meters.

The present invention has at least several advantages over prior artposition estimation methods and systems. The present invention requiresthe range measurement from only one transmitter at a known location (asopposed to the three typically required) and an inertial navigationsystem (which typically exists in most mobile devices such as mobilephones). This invention improves upon the prior art by using thepredicted mobile device position from the INS data during the timeupdate of a loosely coupled Extended Kalman Filter to determine which ofthe two mobile device estimates produced from the One-Step Movementcalculation is correct. Then, the corrected position estimate—the firstupdated position (x, y)—may be used to correct inertial navigationsystem errors in the measurement update phase of the loosely coupledExtended Kalman Filter.

From the above description of the present invention, it is manifest thatvarious techniques may be used for implementing its concepts withoutdeparting from the scope of the claims. The described embodiments are tobe considered in all respects as illustrative and not restrictive. Themethod disclosed herein may be practiced in the absence of any elementthat is not specifically claimed and/or disclosed herein. It should alsobe understood that the present invention is not limited to theparticular embodiments described herein, but is capable of beingpracticed in many embodiments without departure from the scope of theclaims.

What is claimed is:
 1. A method for estimating a position of a mobiledevice comprising the following steps: initializing at the mobile devicean Extended Kalman Filter; physically moving the mobile device from afirst unknown position (x_(t1), y_(t1)) at a first time t₁ to a secondunknown position (x_(t2), y_(t2)) at a second time t₂; measuring at themobile device a distance traveled d between the first unknown positionand the second unknown position; measuring at the mobile device an angletraveled θ between the first unknown position and the second unknownposition; calculating a first possible position (x₁, y₁) at the firsttime; calculating a second possible position (x₂, y₂) at the secondtime; using the Extended Kalman Filter to calculate a predicted firstunknown position ({circumflex over (x)}, ŷ) at the first time;calculating a first final value according to √{square root over(({circumflex over (x)}−x₁)²+(ŷ−y₁)²)}; calculating a second final valueaccording to √{square root over (({circumflex over (x)}−x₂)²+(ŷ−y₂)²)};upon determining the first final value is less than or equal to thesecond final value, outputting the first possible position (x₁, y₁) asthe updated first position (x, y); and upon determining the first finalvalue is greater than the second final value, outputting the secondpossible position (x₂, y₂) as the updated first position (x, y).
 2. Themethod of claim 1, further comprising the step of determining a firstvalue according to sin θ.
 3. The method of claim 2, further comprisingthe step of upon determining the first value is equal to 0, determininga second value according to cos θ.
 4. The method of claim 3, furthercomprising the step of upon determining the second value is equal to 1,calculating a first x-value x₁ according to the relationship${x_{1} = {x_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2d}}},$ whereinx_(TX) is a coordinate of a transmitter located at (x_(TX), y_(TX)), R₁is a first distance between the transmitter and the first unknownposition, R₂ is a second distance between the transmitter and the secondunknown position.
 5. The method of claim 4, further comprising the stepof upon determining the second value is not equal to 1, calculating thefirst x-value x₁ according to the relationship x₁=x_(TX)+$\frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2d}.$
 6. The method of claim 5,further comprising the step of initializing a second x-value x₂according to the relationship x₂=x₁.
 7. The method of claim 6, furthercomprising the step of calculating a first y-value y₁ according to therelationship y₁=y_(TX)+√{square root over (y_(TX) ²−(y_(TX)²+(x₁−x_(TX))²−R₁ ²))}.
 8. The method of claim 7, further comprising thestep of calculating a second y-value y₂ according to the relationshipy₂=y_(TX)−√{square root over (y_(TX) ²−(y_(TX) ²+(x₂−x_(TX))²−R₁ ²))}.9. The method of claim 8, further comprising the step of upondetermining the first value is not equal to 0, calculating a third valueA according to the relationship a=1+cot² θ.
 10. The method of claim 9,further comprising the step of calculating a fourth value b according tothe relationship$b = {{{- 2}x_{TX}} - {2\;\cot\;{\theta\left( {{{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}} \right)}} + {2{y_{TX} \cdot \cot}\;{\theta.}}}$11. The method of claim 10, further comprising the step of calculating afifth value c according to the relationship$c = {x_{TX}^{2} + \left( {{{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}} \right)^{2} - {2{y_{TX}\left( {{{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}} \right)}} + y_{TX}^{2} - {R_{1}^{2}.}}$12. The method of claim 11, further comprising the step of calculatingthe first x-value x₁ according to the relationship$x_{1} = {\frac{{- b} + \sqrt{b^{2} - {4{a \cdot c}}}}{2a}.}$
 13. Themethod of claim 12, further comprising the step of calculating thesecond x-value x₂ according to the relationship$x_{2} = {\frac{{- b} - \sqrt{b^{2} - {4{a \cdot c}}}}{2a}.}$
 14. Themethod of claim 13, further comprising the step of calculating the firsty-value y₁ according to the relationship$y_{1} = {{{{- x_{1}} \cdot \cot}\;\theta} + {{x_{TX} \cdot \cot}\;\theta} + y_{TX} - {\frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}.}}$15. The method of claim 14, further comprising the step of calculatingthe second y-value y₂ according to the relationship$y_{2} = {{{{- x_{2}} \cdot \cot}\;\theta} + {{x_{TX} \cdot \cot}\;\theta} + y_{TX} - {\frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}.}}$16. A method for estimating a position of a mobile device comprising thefollowing steps: initializing at the mobile device an Extended KalmanFilter; physically moving the mobile device from a first unknownposition (x_(t1), y_(t1)) at a first time t₁ to a second unknownposition (x_(t2), y_(t2)) at a second time t₂; measuring at the mobiledevice a distance traveled d between the first unknown position and thesecond unknown position; measuring at the mobile device an angletraveled θ between the first unknown position and the second unknownposition; determining a first value according to sin θ; upon determiningthe first value is equal to 0, determining a second value according tocos θ; upon determining the second value is equal to 1, calculating afirst x-value x₁ according to the relationship${x_{1} = {x_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2d}}},$ whereinx_(TX) is a coordinate of a transmitter located at (x_(TX), y_(TX)), R₁is a first distance between the transmitter and the first unknownposition, R₂ is a second distance between the transmitter and the secondunknown position; upon determining the second value is not equal to 1,calculating the first x-value x₁ according to the relationship${x_{1} = {x_{TX} + \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2d}}};$initializing a second x-value x₂ according to the relationship x₂=x₁;calculating a first y-value y₁ according to the relationshipy ₁ =y _(TX)+√{square root over (y _(TX) ²−(y _(TX) ²+(x ₁ −x _(TX))² −R₁ ²))}; calculating a second y-value y_(z) according to the relationshipy ₂ =y _(TX)−√{square root over (y _(TX) ²−(y _(TX) ²+(x ₂ −x _(TX))² −R₁ ²))}; upon determining the first value is not equal to 0, calculatinga third value a according to the relationship a=1+cot² θ; calculating afourth value b according to the relationship${b = {{{- 2}x_{TX}} - {2\;\cot\;{\theta\left( {{{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}} \right)}} + {2{y_{TX} \cdot \cot}\;\theta}}};$calculating a fifth value c according to the relationship${c = {x_{TX}^{2} + \left( {{{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}} \right)^{2} - {2{y_{TX}\left( {{{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}} \right)}} + y_{TX}^{2} - R_{1}^{2}}};$calculating the first x-value x₁ according to the relationship${x_{1} = \frac{{- b} + \sqrt{b^{2} - {4{a \cdot c}}}}{2a}};$calculating the second x-value x₂ according to the relationship${x_{2} = \frac{{- b} - \sqrt{b^{2} - {4{a \cdot c}}}}{2a}};$calculating the first y-value y₁ according to the relationship${y_{1} = {{{{- x_{1}} \cdot \cot}\;\theta} + {{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}}};$calculating the second y-value y₂ according to the relationship${y_{2} = {{{{- x_{2}} \cdot \cot}\;\theta} + {{x_{TX} \cdot \cot}\;\theta} + y_{TX} - \frac{R_{1}^{2} + d^{2} - R_{2}^{2}}{2{d \cdot \sin}\;\theta}}};$using the Extended Kalman Filter to calculate a predicted first unknownposition ({circumflex over (x)}, ŷ) at the first time; calculating afirst final value according to √{square root over (({circumflex over(x)}−x₁)²+(ŷ−y₁)²)}; calculating a second final value according to√{square root over (({circumflex over (x)}−x₂)²+(ŷ−y₂)²)}; upondetermining the first final value is less than or equal to the secondfinal value, outputting the first possible position (x₁, y₁) as theupdated first position (x, y); and upon determining the first finalvalue is greater than the second final value, outputting the secondpossible position (x₂, y₂) as the updated first position (x, y).